home *** CD-ROM | disk | FTP | other *** search
- #define DRIVE OUT_A
- #define STEER OUT_B
- #define CLICK OUT_C
-
- #define STEER_POS SENSOR_1
- #define DRIVE_POS SENSOR_2
-
- #define CLICK_DOWN_TIME 30
- #define CLICK_UP_TIME 20
-
- int goal;
-
- task main()
- {
- SetSensor(STEER_POS, SENSOR_ROTATION);
- SetSensor(DRIVE_POS, SENSOR_ROTATION);
-
- ClearMessage();
- until(Message()==1);
-
- // launch
- click();
- click();
-
- Wait(100);
-
- // v move
- goal = -4;
- drive();
- click();
-
- // h move
- goal = -28;
- steer();
- goal = -15;
- drive();
-
- goal = 0;
- steer();
- goal = 4;
- drive();
-
- click();
- }
-
- sub drive()
- {
- ClearSensor(DRIVE_POS);
-
- if (goal < 0)
- {
- Fwd(DRIVE);
- On(DRIVE);
- until(DRIVE_POS <= goal);
- }
- else if (goal > 0)
- {
- Rev(DRIVE);
- On(DRIVE);
- until(DRIVE_POS >= goal);
- }
-
- Off(DRIVE);
- }
-
-
- sub steer()
- {
- int p;
-
- On(STEER);
- while(true)
- {
- p = STEER_POS;
- if (p == goal)
- {
- Off(STEER);
- return;
- }
- else if (p > goal)
- {
- Fwd(STEER);
- }
- else
- {
- Rev(STEER);
- }
- }
- }
-
- sub click()
- {
- OnFwd(CLICK);
- Wait(CLICK_DOWN_TIME);
- Rev(CLICK);
- Wait(CLICK_UP_TIME);
- Off(CLICK);
- }
-
-
- sub init()
- {
- }